#include "readyqueue.h"

static struct task *readyqueue [READYQUEUESIZE + 1];
static struct task *idle;
static unsigned int head = 0;
static unsigned int tail = 0;
static unsigned int count = 0;

int RQenqueue (struct task *task)
{
	if (count >= READYQUEUESIZE)
	{
		return 0; // error
	}
	if (task->id)
	{
		readyqueue [tail] = task;
		tail = (tail + 1) & READYQUEUESIZE;
		count ++;
	}
	else
	{
		idle = task;
	}
	return 1;
}

int RQdequeue (void)
{
	if (count == 0)
	{
		return 0; // error
	}
	head = (head + 1) & READYQUEUESIZE;
	count --;
	return 1;
}

#if 1
struct task *RQtop (void)
{
	struct task *task;
	int i;
	if (count == 0)
	{
		return 0;
	}
	for (i = 0; i < count; i ++)
	{
		task = readyqueue [head];
		RQdequeue ();
		RQenqueue (task);
		if ((semaphoreState () & task->wait) == task->wait)
		{
			return task;
		}
		else if (task->wait <= getTickCount ())
		{
			return task;
		}
	}
	return idle;
}
#else
struct task *RQtop (void)
{
	struct task *task;
	if (count == 0)
	{
		return 0;
	}
	task = readyqueue [head];
	RQdequeue (); // dequeue first, so as to avoid overflows
	RQenqueue (task);
	if (task->id == 0) // it is the idle task
	{
		return task; // the idle task never blocks
	}
	else if ((semaphoreState () & task->wait) == task->wait) // any semaphores the task needs are set
	{
		return task;
	}
	else if (task->wait <= getTickCount ()) // the task has waited long enough
	{
		return task;
	}
	else
	{
		return RQtop ();
	}
}
#endif 

